#include <glib/g3/trimesh.h>
#include <glib/g3/algorithm.h>

#include <map>
#include <sstream>
#include <limits>
#include <vector>

namespace g3
{

bool compute_topology(const trimesh & m, trimesh_topology & t, glib::progress_callback * pcb)
{
	if (pcb != 0 && !pcb->on_progress(0.0))
		return false;

	t.edges.clear();
	t.faces.clear();

	t.faces.resize(m.faces.size());
	t.edges.reserve(m.coords.size() * 3);

	typedef std::map<edge, int> edge_map;
	edge_map edge_indices;

	int percent = -1;
	int done = 0;

	for (size_t f = 0; f < m.faces.size(); ++f)
	{
		const g3::face & face = m.faces[f];

		face_info & finfo = t.faces[f];

		int orient;

		for (int i = 0; i < 3; ++i)
		{
			g3::edge edge = orient_edge(face.v[i], face.v[(i + 1) % 3], orient);

			edge_map::iterator eit = edge_indices.lower_bound(edge);
			if (eit == edge_indices.end() 
				|| eit->first != edge)
			{
				// new edge
				int e = int(t.edges.size());
				edge_indices.insert(eit, std::make_pair(edge, e));
				t.edges.resize(t.edges.size() + 1);
				edge_info & einfo = t.edges.back();
				einfo.f[0] = f;
				einfo.e[0] = e;
				finfo.e[i] = e;
				finfo.f[i] = 0;
			}
			else
			{
				// existing edge
				int e = eit->second;
				edge_info & einfo = t.edges[e];
				if (einfo.f[1] != -1)
				{
					std::ostringstream out;
					out << "Bad edge #" << e; 
					throw std::runtime_error(out.str());
				}
				einfo.f[1] = f;
				einfo.e[1] = e;
				finfo.e[i] = e;
				finfo.f[i] = 1;
			}
		}

		if (pcb != 0)
		{
			int p = int(100 * double(++done) / m.faces.size());
			if (p != percent) 
			{
				percent = p;
				if (!pcb->on_progress(float(percent) * 0.01f))
					return false;
			}
		}
	}

	return true;
}

void compute_normals(const trimesh & mesh, std::vector<vector> & normals)
{
	normals.clear();
	normals.resize(mesh.coords.size(), vector(0, 0, 0));
	for (size_t i = 0; i < mesh.faces.size(); ++i)
	{
		const face & f = mesh.faces[i];
		const vector & p0 = mesh.coords.at(f.v[0]);
		const vector & p1 = mesh.coords.at(f.v[1]);
		const vector & p2 = mesh.coords.at(f.v[2]);
		vector n = tri_normal(p0, p1, p2);
		normals[f.v[0]] += n;
		normals[f.v[1]] += n;
		normals[f.v[2]] += n;
	}
	for (size_t i = 0; i < normals.size(); ++i)
	{
		double n = abs(normals[i]);
		if (n != 0.0)
			normals[i] /= n;
	}
}

void build_vertex_grid(const std::vector<vector> & vv, hash_grid<int> & hg, double cell_size)
{
	hg.reset(cell_size);
	for (size_t i = 0; i < vv.size(); ++i)
	{
		const vector & v = vv[i];
		hg.insert(box(v, v), i);
	}
}

void build_face_grid(const std::vector<vector> & vv, const std::vector<face> & ff, hash_grid<int> & hg, double cell_size)
{
	hg.reset(cell_size);
	for (size_t i = 0; i < ff.size(); ++i)
	{
		const face & f = ff[i];
		box b(vv.at(f.v[0]), vv.at(f.v[0]));
		b.include(vv.at(f.v[1]));
		b.include(vv.at(f.v[2]));
		hg.insert(b, i);
	}
}

bool project_point_on_surface(const g3::vector & v, const trimesh & s, 
							  const hash_grid<int> & face_grid, double max_distance, 
							  int & face_index, vector & bary_coords)
{
	std::vector<char> checked(s.faces.size(), 0);

	box b(v, v);
	b.inflate(max_distance);
	hash_grid<int>::range_iterator rit;
	
	double dmin = std::numeric_limits<double>::max();
	face_index = -1;

	for (rit = face_grid.range_begin(b); !rit.end(); ++rit)
	{
		int fi = * rit;
		if (checked[fi] == 1)
			continue;

		checked[fi] = 1;

		const g3::face & f = s.faces.at(fi);

		const vector & q0 = s.coords.at(f.v[0]);
		const vector & q1 = s.coords.at(f.v[1]);
		const vector & q2 = s.coords.at(f.v[2]);
		bool degenerate;
		double h, b[3];
		double d = distance_to_triangle(v, q0, q1, q2, degenerate, h, b);
		if (d < dmin)
		{
			face_index = * rit;
			bary_coords(0) = b[0];
			bary_coords(1) = b[1];
			bary_coords(2) = b[2];
			dmin = d;
		}
	}

	return face_index != -1 && dmin <= max_distance;
}

void simple_smooth(trimesh & s, double alpha)
{
	std::vector<vector> new_coords = s.coords;
	std::vector<size_t> weights(s.coords.size(), 1);
	for (size_t i = 0; i < s.faces.size(); ++i)
	{
		const face & f = s.faces[i];
		new_coords[f.v[1]] += s.coords[f.v[0]];
		++ weights[f.v[1]];
		new_coords[f.v[2]] += s.coords[f.v[1]];
		++ weights[f.v[2]];
		new_coords[f.v[0]] += s.coords[f.v[2]];
		++ weights[f.v[0]];
	}
	for (size_t i = 0; i < new_coords.size(); ++i)
	{
		new_coords[i] = s.coords[i] * (1 - alpha) + alpha * (new_coords[i] / double(weights[i]));
	}
	s.coords.swap(new_coords);
}

void simple_smooth(trimesh & s, double alpha, const std::vector<bool> & mask)
{
	std::vector<vector> new_coords = s.coords;
	std::vector<size_t> weights(s.coords.size(), 1);
	for (size_t i = 0; i < s.faces.size(); ++i)
	{
		const face & f = s.faces[i];
		if (mask[f.v[1]])
		{
			new_coords[f.v[1]] += s.coords[f.v[0]];
			++ weights[f.v[1]];
		}
		if (mask[f.v[2]])
		{
			new_coords[f.v[2]] += s.coords[f.v[1]];
			++ weights[f.v[2]];
		}
		if (mask[f.v[0]])
		{
			new_coords[f.v[0]] += s.coords[f.v[2]];
			++ weights[f.v[0]];
		}
	}
	for (size_t i = 0; i < new_coords.size(); ++i)
		if (mask[i])
			new_coords[i] = s.coords[i] * (1 - alpha) + alpha * (new_coords[i] / double(weights[i]));
	s.coords.swap(new_coords);
}

} // g3